#ifndef ARMMOVER_H_
#define ARMMOVER_H_

#include "ros/ros.h"
//
//#include "ros/publisher.h"
//#include "ros/node_handle.h"
//#include "ros/rate.h"

#include "std_msgs/Float64.h"


/**
 * Publishes movement operations for the robot's rover unit
 */
class ArmMover {

private:
	ros::NodeHandle m_nodeHandler;
	ros::Rate m_loop_rate;

	// Publishers
	ros::Publisher m_arm_upper_elbow_pub;
	ros::Publisher m_arm_lower_elbow_pub;
	ros::Publisher m_arm_shoulder_pub;
	ros::Publisher m_arm_wrist_pub;
	ros::Publisher m_arm_base_rotation_pub;
	ros::Publisher m_arm_left_finger_pub;
	ros::Publisher m_arm_right_finger_pub;

	// Messages (maybe need only one)
	std_msgs::Float64 m_arm_upper_elbow_msg;
	std_msgs::Float64 m_arm_lower_elbow_msg;
	std_msgs::Float64 m_arm_shoulder_msg;
	std_msgs::Float64 m_arm_wrist_msg;
	std_msgs::Float64 m_arm_base_rotation_msg;
	std_msgs::Float64 m_arm_left_finger_msg;
	std_msgs::Float64 m_arm_right_finger_msg;

public:
	ArmMover();
	void DoCheck();
	void MoveUpperElbow(double value);
	void MoveLowerElbow(double value);
	void MoveShoulder(double value);
	void MoveWrist(double value);
	void RotateBase(double value);
	void MoveLeftFinger(double value);
	void MoveRightFinger(double value);

	void MoveShoulderAndUpperElbow(double shoulder_value,double upper_elbow_value);


};

#endif

